covariance.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
#include <Eigen/Dense>
 
namespace wolf
{
template <typename T, int N, int RC>
inline bool isSymmetric(const Eigen::Matrix<T, N, N, RC>& M, const T eps = wolf::Constants::EPS)
{
    if (M.cols() != M.rows()) return false;
    return M.isApprox(M.transpose(), eps);
}
 
template <typename T, int N, int RC>
inline bool isPositiveSemiDefinite(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
{
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M, Eigen::EigenvaluesOnly);
 
    if (eigensolver.info() == Eigen::Success)
    {
        // All eigenvalues must be >= 0:
        return (eigensolver.eigenvalues().array() >= eps).all();
    }
    else
        std::cout << "eigen decomposition failed" << std::endl;
 
    return false;
}
 
template <typename T, int N, int RC>
inline bool isCovariance(const Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
{
    return isSymmetric(M) && isPositiveSemiDefinite(M, eps);
}
 
#define WOLF_ASSERT_COVARIANCE_MATRIX(x) assert(isCovariance(x, Constants::EPS_SMALL) && "Not a covariance");
 
#define WOLF_ASSERT_INFORMATION_MATRIX(x) assert(isCovariance(x, double(0.0)) && "Not an information matrix");
 
template <typename T, int N, int RC>
inline bool makePosDef(Eigen::Matrix<T, N, N, RC>& M, const T& eps = Constants::EPS)
{
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N, RC> > eigensolver(M);
 
    if (eigensolver.info() == Eigen::Success)
    {
        // All eigenvalues must be >= 0:
        double epsilon = eps;
        while ((eigensolver.eigenvalues().array() < eps).any())
        {
            // std::cout << "----- any negative eigenvalue or too close to zero\n";
            // std::cout << "previous eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
            // std::cout << "previous determinant: " << M.determinant() << std::endl;
            M = eigensolver.eigenvectors() * eigensolver.eigenvalues().cwiseMax(epsilon).asDiagonal() *
                eigensolver.eigenvectors().transpose();
            eigensolver.compute(M);
            // std::cout << "epsilon used: " << epsilon << std::endl;
            // std::cout << "posterior eigenvalues: " << eigensolver.eigenvalues().transpose() << std::endl;
            // std::cout << "posterior determinant: " << M.determinant() << std::endl;
            epsilon *= 10;
        }
        WOLF_ASSERT_COVARIANCE_MATRIX(M);
 
        return epsilon != eps;
    }
    else
        WOLF_ERROR("Couldn't compute covariance eigen decomposition");
 
    return false;
}
 
inline Eigen::MatrixXd computeSqrtUpper(const Eigen::MatrixXd& _info)
{
    // impose symmetry
    Eigen::MatrixXd info = _info.selfadjointView<Eigen::Upper>();
 
    // Normal Cholesky factorization
    Eigen::LLT<Eigen::MatrixXd> llt_of_info(info);
    Eigen::MatrixXd             R = llt_of_info.matrixU();
 
    // Good factorization
    if (info.isApprox(R.transpose() * R, Constants::EPS)) return R;
 
    // Not good factorization: SelfAdjointEigenSolver
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(info);
    Eigen::VectorXd                                eval = es.eigenvalues().real().cwiseMax(Constants::EPS);
 
    R = eval.cwiseSqrt().asDiagonal() * es.eigenvectors().real().transpose();
 
    return R;
}
 
}  // namespace wolf